#include <rclcpp/rclcpp.hpp>
// #include <tf2_msgs/msg/tf_message.hpp>
#include <tf2/LinearMath/Quaternion.h>
// #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

bool isSameEuler(const std::vector<double> &goal_pose, const std::vector<double> &cur_pose)
{
    double pose_tolerance = sqrt(pow(fabs(goal_pose[0]) - fabs(cur_pose[0]), 2) + pow(fabs(goal_pose[1]) - fabs(cur_pose[1]), 2) + pow(fabs(goal_pose[2]) - fabs(cur_pose[2]), 2));
    std::cout << "pose tolerance is: " << pose_tolerance << std::endl;
    // yaw、pitch 和 roll 分别是绕 $Z$、$Y$ 和 $X$ 轴的旋转角度，单位为弧度。
    tf2::Quaternion goal_orientation, cur_orientation;
    goal_orientation.setEuler(goal_pose[3], goal_pose[4], goal_pose[5]);
    cur_orientation.setEuler(cur_pose[3], cur_pose[4], cur_pose[5]);
    double goal_x = goal_orientation.x();
    double goal_y = goal_orientation.y();
    double goal_z = goal_orientation.z();
    double goal_w = goal_orientation.w();
    std::cout << "w1: " << goal_w << std::endl;
    std::cout << "x1: " << goal_x << std::endl;
    std::cout << "y1: " << goal_y << std::endl;
    std::cout << "z1: " << goal_z << std::endl;

    double cur_x = cur_orientation.x();
    double cur_y = cur_orientation.y();
    double cur_z = cur_orientation.z();
    double cur_w = cur_orientation.w();
    std::cout << "w2: " << cur_w << std::endl;
    std::cout << "x2: " << cur_x << std::endl;
    std::cout << "y2: " << cur_y << std::endl;
    std::cout << "z2: " << cur_z << std::endl;

    double orientation_tolerance = sqrt(pow(fabs(goal_x) - fabs(cur_x), 2) + pow(fabs(goal_y) - fabs(cur_y), 2) + pow(fabs(goal_z) - fabs(cur_z), 2) + pow(fabs(goal_w) - fabs(cur_w), 2));
    std::cout << "orientation tolerance: " << orientation_tolerance << std::endl;

    return pose_tolerance < 0.1 && orientation_tolerance < 0.1;
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    // tf2::Quaternion orientation1, orientation2;
    // orientation1.setRPY(111.763063 * M_PI / 180, -152.095440 * M_PI / 180, 4.662663 * M_PI / 180);
    // std::cout << "w1: " << orientation1.w() << std::endl;
    // std::cout << "x1: " << orientation1.x() << std::endl;
    // std::cout << "y1: " << orientation1.y() << std::endl;
    // std::cout << "z1: " << orientation1.z() << std::endl;
    // std::cout << std::endl;

    // orientation2.setRPY(-68.237000 * M_PI / 180, -27.904543 * M_PI / 180, -175.337317 * M_PI / 180);
    // std::cout << "w2: " << orientation2.w() << std::endl;
    // std::cout << "x2: " << orientation2.x() << std::endl;
    // std::cout << "y2: " << orientation2.y() << std::endl;
    // std::cout << "z2: " << orientation2.z() << std::endl;

    std::vector<double> goal_pose{683.620394, 342.955081, 639.901922, 111.763063 * M_PI / 180, -152.095440 * M_PI / 180, 4.662663 * M_PI / 180};
    std::vector<double> cur_pose{683.620752, 342.954975, 639.901772, -68.237000 * M_PI / 180, -27.904543 * M_PI / 180, -175.337317 * M_PI / 180};
    if (isSameEuler(goal_pose, cur_pose))
    {
        std::cout << "yes" << std::endl;
    }
    else
    {
        std::cout << "no" << std::endl;
    }

    return 0;
}